import matplotlib.pyplot as plt
import numpy as np
import math

T = 10
delta = 0.1
XK = 1
K = list(range(1,int(T/ delta) + 1))
t = [0] * len(K)
wk = [0] * len(K)
x_measure = [0] * len(K)
x_hat_zero = 0
x_hat_first = [0] * len(K)
x_hat_second = [0] * len(K)

for i in range(0, len(K)):
    t[i] = (K[i] - 1) * delta
    wk[i] = np.random.normal(loc=0.0, scale=1.0, size=None)
    x_measure[i] = XK + wk[i]

#0阶滤波器的估计值
x_hat_zero = (1 / len(K)) * sum(x_measure)

#一阶滤波器的估计值
#一阶最小二乘滤波器
m1 = np.array([[len(K),sum(t)],[sum(t),sum(list(map(lambda x:x**2,t)))]])
                                     #最后一个求和符号里面是求数列个元素平方
w = [t*x_measure for t,x_measure in zip(t,x_measure)]
m2 = np.array([[sum(x_measure)],[sum(w)]])

a = (np.linalg.inv(m1)).dot(m2)      #m1求逆再乘以m2
a0 = float(a[0])
a1 = float(a[1])

for i in range(0, len(K)):
    x_hat_first[i] = a0 + t[i] * a1

#二阶最小二乘滤波器的估计值
#二阶最小二乘滤波器,估计值的计算
m1 = np.array([[4,sum(t),sum(list(map(lambda x:x**2,t)))]
                ,[sum(t),sum(list(map(lambda x:x**2,t))),sum(list(map(lambda x:x**3,t)))]
                ,[sum(list(map(lambda x:x**2,t))),sum(list(map(lambda x:x**3,t))),sum(list(map(lambda x:x**4,t)))]
              ])
                                     #最后一个求和符号里面是求数列个元素平方
w1 = [t*x_measure for t,x_measure in zip(t,x_measure)]
                                     #t和x逐项相乘
t_2 = list(map(lambda x:x**2,t))
w2 = [t_2*x_measure for t_2,x_measure in zip(t_2,x_measure)]
m2 = np.array([[sum(x_measure)],[sum(w1)],[sum(w2)]])

a = (np.linalg.inv(m1)).dot(m2)      #m1求逆再乘以m2
a0 = float(a[0])
a1 = float(a[1])
a2 = float(a[2])
for i in range(0,len(K)):
    x_hat_second[i] = a2 + a1 * t[i] + a2 * (t[i] ** 2)

plt.xlabel('Time(Sec)')
plt.ylabel('Estimate of x')
plt.axhline(y = XK, c = "k", label = 'true', ls = "--", lw = 2)
plt.axhline(y = x_hat_zero, c = "r", label = 'zero_order', ls = "--", lw = 2)
plt.plot(t, x_hat_first, c = "g", label = 'first_order', ls = "--", lw = 2)
plt.plot(t, x_hat_second, c = "k", label = 'second_order', ls = "--", lw = 2)
plt.legend('Zero_order_singal')
plt.show()